% clc;clear;close all;
% load('testdata_gardner_5.mat');
PLOT_LEN = 1200;
ZERO_LEN = 0;%0 is the best fine tune

% signal_rx = floor(signal(1:20000));
% signal_rx = repmat(signal_rx,1,25);
% signal_rx = reshape(signal_rx,[],1);
% figure(1);subplot(311);plot((0:PLOT_LEN-1)/60e6,imag(signal_rx(1:PLOT_LEN)));
% figure(2);subplot(311);plot((0:PLOT_LEN-1)/60e6,real(signal_rx(1:PLOT_LEN)));
% 
% signal_rx = downsample([zeros(ZERO_LEN,1);signal_rx], 6);
% signal_rx(1:51) = 0;
% figure(1);subplot(312);plot((0:PLOT_LEN/6-1)/10e6,imag(signal_rx(1:PLOT_LEN/6)),'-*');
% figure(2);subplot(312);plot((0:PLOT_LEN/6-1)/10e6,real(signal_rx(1:PLOT_LEN/6)),'-*');
% 
% rcos_fir = round(1.15 * rcosdesign(0.5, 6, 4, 'sqrt') * 2^15);
% signal_rx = floor(filter(rcos_fir, 1, signal_rx) / (2^16));
% signal_rx = [0;signal_rx];
% figure(1);subplot(313);plot((0:PLOT_LEN/6-1)/10e6,imag(signal_rx(1:PLOT_LEN/6)),'-*');
% figure(2);subplot(313);plot((0:PLOT_LEN/6-1)/10e6,real(signal_rx(1:PLOT_LEN/6)),'-*');

%% Carrier Synchronization and Bit Synchronization
% using PLL and Gardner Loop
signal_rx = pll.watch.output;
GardnerParams = GardnerInit();
for index_data = 1:length(signal_rx)
    % calculation Gardner nco
    GardnerParams.nco(2) = GardnerParams.nco(1) - GardnerParams.w_fifo(1);

    % load pll demodulation data for farrow interpolation
    % GardnerParams.data_for_farrow(1) is the latest data
    GardnerParams.data_for_farrow = [signal_rx(index_data); GardnerParams.data_for_farrow(1:end - 1)];
    % Gardner farrow interpolation
    FI1 = floor(0.5 * GardnerParams.data_for_farrow(1)) - floor(0.5 * GardnerParams.data_for_farrow(2)) ...
    - floor(0.5 * GardnerParams.data_for_farrow(3)) + floor(0.5 * GardnerParams.data_for_farrow(4));
    FI2 = GardnerParams.data_for_farrow(2) + floor(0.5 * GardnerParams.data_for_farrow(2)) ...
        - floor(0.5 * GardnerParams.data_for_farrow(1)) - floor(0.5 * GardnerParams.data_for_farrow(3)) ...
        - floor(0.5 * GardnerParams.data_for_farrow(4));
    FI3 = GardnerParams.data_for_farrow(3);
    temp1 = floor(FI1 * GardnerParams.u / (2^15));
    temp2 = floor(temp1 * GardnerParams.u / (2^15));
    temp3 = floor(FI2 * GardnerParams.u / (2^15));


    % error calculation should be implemented if nco overflow
    if (GardnerParams.nco(2) <= 0)

            % load farrow interpolation result into farrow_output array
        GardnerParams.farrow_output = [temp2 + temp3 + FI3; GardnerParams.farrow_output(1:end - 1)];
        GardnerParams.farrow_num = GardnerParams.farrow_num + 1;

        % add farrow output to GardnerParams.watch
        GardnerParams.watch.farrow_output = [GardnerParams.watch.farrow_output;GardnerParams.farrow_output(1)];

        % error calculation
        if (mod(GardnerParams.farrow_num, 2) == 0)
            err_real = real(GardnerParams.farrow_output(1)) - real(GardnerParams.farrow_output(3));
            err_imag = imag(GardnerParams.farrow_output(1)) - imag(GardnerParams.farrow_output(3));
            err_mul_real = real(GardnerParams.farrow_output(2)) * err_real;
            err_mul_imag = imag(GardnerParams.farrow_output(2)) * err_imag;

            GardnerParams.time_error = [err_mul_real + err_mul_imag;GardnerParams.time_error(1)];
            clear err_real err_imag err_mul_real err_mul_imag;

            % second order loop filter
            GardnerParams.err_filter = GardnerParams.err_filter + GardnerParams.time_error(1);
            GardnerParams.w_delta = floor(GardnerParams.err_filter / (2^21)) + floor(GardnerParams.time_error(1) / (2^14));
            GardnerParams.w = 16370 + GardnerParams.w_delta;
            GardnerParams.w_fifo = [GardnerParams.w_fifo(2:end);GardnerParams.w];

            % add error and w to watch
            GardnerParams.watch.time_error = [GardnerParams.watch.time_error GardnerParams.time_error(1)];
            GardnerParams.watch.w = [GardnerParams.watch.w GardnerParams.w];
            GardnerParams.watch.w_delta = [GardnerParams.watch.w_delta GardnerParams.w_delta];
            GardnerParams.watch.err_filter = [GardnerParams.watch.err_filter,GardnerParams.err_filter];
        end

        % update u
        GardnerParams.u = floor(GardnerParams.nco(1) * 2);
        GardnerParams.watch.u = [GardnerParams.watch.u;GardnerParams.u];

        GardnerParams.nco(2) = mod(GardnerParams.nco(2), 2^15);
    end

    GardnerParams.nco(1) = GardnerParams.nco(2);
    GardnerParams.watch.nco = [GardnerParams.watch.nco GardnerParams.nco(2)];
end

%% ------------------------------Debug-----------------------------------------
GardnerParams.output = GardnerParams.watch.farrow_output(2:2:end);
figure;plot(signal_rx,'r*');hold on;
plot(GardnerParams.output(4000:end), 'bs');hold off;
figure; plot(real(GardnerParams.watch.farrow_output));
hold on; plot(real(GardnerParams.output(1000:end)),'r*');hold off;
figure;plot(GardnerParams.watch.err_filter)
figure;plot(GardnerParams.watch.u)